//
// Created by nsy on 2020/11/20.
//

#include <filters/voxel_grid_filter.h>
#include "../../include/shader/color_mapping.h"


ColorMapping::ColorMapping() {
    resolution_ = 0.05f;
}

/**
 * 设置输入点云
 * @param[input] cloud
 * @return
 */
void ColorMapping::setInputCloud(const CommonShader::PointCloudPtr cloud) {

    cloud_ = cloud;
    
}

/**
 * 设置相机
 * @param[input] camera
 * @return
 */
void ColorMapping::setCamera(const CameraInfo camera) {

    camera_ = camera;
    
}

/**
 * 颜色映射
 * @param[output] out
 * @param[input] check_occluded 是否检测阻挡，默认不检测
 * @return
 */
void ColorMapping::colorMapping(ColorMapping::PointCloudRGB &out,
                                           bool check_occluded) {

    // 前置校验
    if (cloud_ == nullptr) {
        PCL_ERROR("未设置点云");
        
    }
    else if (camera_.getWidth() <= 0) {
        PCL_ERROR("相机图片信息有误");
        
    }

    // 读取图片
    cv::Mat image = cv::imread(camera_.getImagePath());
    // 根据相机位姿旋转点云到相机坐标系下
    PointCloudPtr cloud_camera(new PointCloud);
    transformCloudByCamera(*cloud_, *cloud_camera, camera_);

    // 生成八叉树，用于阻挡检测
    OctreePtr octree(new Octree(resolution_));
    if (check_occluded) {
        octree->setInputCloud(cloud_camera);
        octree->addPointsFromInputCloud();
    }

    // 复制点云xyz信息
    pcl::copyPointCloud(*cloud_, out);

    // 获取每个点的颜色
    for (int i = 0; i < cloud_camera->size(); ++i) {
        Point p = cloud_camera->at(i);

        Eigen::Vector2i v;
        if (check_occluded && isPointOccluded(p, octree)) {
            // 需要计算阻挡并且点被其他点阻挡了，设置点为(-1,-1)
            v << -1, -1;
        }
        else {
            // 没被阻挡或者不需要处理阻挡，获取点的平面坐标
            getXYCoordinates(p, camera_, v);
        }

        int y = v.x();
        int x = v.y();
        if(x >= 0 && y >= 0 && y < camera_.getWidth() && x < camera_.getHeight()) {
            // 点在相机视角内，获取点的rgb值
            out.at(i).r = (double) image.at<cv::Vec3b>(x, y)[2];
            out.at(i).g = (double) image.at<cv::Vec3b>(x, y)[1];
            out.at(i).b = (double) image.at<cv::Vec3b>(x, y)[0];
        }
        else {
            // 不在视角内，设置点为白色
            out.at(i).r = out.at(i).g = out.at(i).b = 255;
        }

    }

    
}


/**
 * 添加相机信息
 * @param[input] camera
 * @return
 */
void ColorMapping::pushBackCamera(const CameraInfo camera) {

    camera_list_.push_back(camera);
    
}


/**
 * 设置相机列表
 * @param[input] camera_list
 * @return
 */
void ColorMapping::setCameraList(ColorMapping::CameraList camera_list) {

    camera_list_ = camera_list;
    
}


/**
 * 多相机颜色映射
 * @param[output] out
 * @return
 */
void ColorMapping::colorMappingMultiCamera(ColorMapping::PointCloudRGB &out) {

    // 距离数组，记录各个相机到点云各点的距离及各点在对应相机中的颜色
    std::vector<std::vector<Distance>> distance_list(camera_list_.size());

    // 遍历相机 记录距离以及点颜色
    for (int camera_index = 0; camera_index < camera_list_.size(); ++camera_index) {

        // 将点云变换到相机坐标系下
        PointCloudPtr cloud_camera(new PointCloud);
        transformCloudByCamera(*cloud_, *cloud_camera, camera_list_[camera_index]);

        // 生成八叉树，用于检测阻挡
        OctreePtr octree(new Octree(resolution_));
        octree->setInputCloud(cloud_camera);
        octree->addPointsFromInputCloud();

        // 读取相机照片
        cv::Mat image = cv::imread(camera_list_[camera_index].getImagePath());

        // 遍历点云中的点
        for (int j = 0; j < cloud_camera->size(); ++j) {
            Point p = cloud_camera->at(j);
            Distance distance{};
            if (isPointOccluded(p, octree)) {
                // 点被阻挡
                distance.distance = __DBL_MAX__;
                distance.r = distance.g = distance.b = 255;
            }
            else {
                Eigen::Vector2i coordinates;
                getXYCoordinates(p, camera_list_[camera_index], coordinates, distance.distance);
                int y = coordinates.x();
                int x = coordinates.y();
                if(x >= 0 && y >= 0 && y < image.cols && x < image.rows) {
                    // 点在相机视角内，获取点的rgb值
                    distance.r = (double) image.at<cv::Vec3b>(x, y)[2];
                    distance.g = (double) image.at<cv::Vec3b>(x, y)[1];
                    distance.b = (double) image.at<cv::Vec3b>(x, y)[0];
                }
                else {
                    // 不在视角内，设置点为白色
                    distance.r = distance.g = distance.b = 255;
                }
            }

            distance_list[camera_index].push_back(distance);
        } // end cloud

    } // end camera

    // 根据距离计算最优相机上色
    pcl::copyPointCloud(*cloud_, out);
    for (int i = 0; i < cloud_->size(); ++i) {
        Distance distance = distance_list[0][i];
        for (int j = 1; j < camera_list_.size(); ++j) {
            if (distance.distance > distance_list[j][i].distance) {
                distance = distance_list[j][i];
            }
        }
        out.at(i).r = distance.r;
        out.at(i).g = distance.g;
        out.at(i).b = distance.b;
    }

    
}

